#include "stepper.h"
// #include "pin_mux.h"
#include "fsl_port.h"

struct stepper_device_t motor_x =
    {
        .en_pin = (32 * 1) + 17,
        .dir_pin = (32 * 1) + 1,
        .step_pin = (32 * 1) + 3,
        .timer_device_name = "timer1",
        .pwm_device_name = "pwm1",
        .pwm_channel = 1,
        .cb = motor_x_cb};

struct stepper_device_t motor_y =
    {
        .en_pin = (32 * 3) + 22,
        .dir_pin = (32 * 3) + 21,
        .step_pin = (32 * 1) + 12,
        .timer_device_name = "timer2",
        .pwm_device_name = "pwm2",
        .pwm_channel = 2,
        .cb = motor_y_cb};

void stepper_init(struct stepper_device_t *ins)
{
    // 1. Initial sw & hw
    rt_pin_mode(ins->en_pin, PIN_MODE_OUTPUT);
    rt_pin_mode(ins->dir_pin, PIN_MODE_OUTPUT);

    rt_pin_write(ins->en_pin, PIN_HIGH);
    rt_pin_write(ins->dir_pin, PIN_LOW);

    rt_hwtimerval_t timeout;
    rt_hwtimer_mode_t mode = HWTIMER_MODE_ONESHOT;
    rt_err_t res;
    uint32_t freq = 150000000;

    ins->motor_lock = 0;
    // 2. Initial HWTIMER

    ins->timer_device = rt_device_find(ins->timer_device_name);
    if (ins->timer_device == NULL)
        rt_kprintf("hwtimer device not found.\r\n");
    res = rt_device_open(ins->timer_device, RT_DEVICE_FLAG_RDWR);
    if (res != RT_EOK)
        rt_kprintf("open hwtimer failed=%d.\r\n", res);

    rt_device_set_rx_indicate(ins->timer_device, ins->cb);
    if (res != RT_EOK)
        rt_kprintf("attatch callback error=%d.\r\n", res);

    rt_device_control(ins->timer_device, HWTIMER_CTRL_FREQ_SET, &freq);
    rt_device_control(ins->timer_device, HWTIMER_CTRL_MODE_SET, &mode);

    // 3. Initial PWM peripheral
    ins->pwm_device = rt_device_find(ins->pwm_device_name);
    if (ins->pwm_device == NULL)
        rt_kprintf("PWM device not found.\r\n");
}

/**
 * @brief
 *
 * @param ins
 * @param speed     rotation speed,step/s.
 * @param duration  duration of motion in us.
 */
void stepper_launch(struct stepper_device_t *ins, int32_t speed, uint32_t duration)
{
    int32_t pulse_period = 1000000000 / abs(speed);
    rt_hwtimerval_t timeout;

    timeout.usec = duration % 1000000;
    timeout.sec = duration / 1000000;
    if (speed == 0 || duration == 0)
    {
        ins->motor_lock = 0;
        return;
    }

    if (speed > 0)
        stepper_dir_set(ins, STEPPER_DIR_CW);
    else
        stepper_dir_set(ins, STEPPER_DIR_CCW);

    rt_pwm_set((struct rt_device_pwm *)ins->pwm_device, ins->pwm_channel, pulse_period, pulse_period / 2);
    rt_pwm_enable((struct rt_device_pwm *)ins->pwm_device, ins->pwm_channel);
    rt_device_write(ins->timer_device, 0, &timeout, sizeof(rt_hwtimerval_t));
}

void stepper_port_init()
{
    PORT_SetPinMux(PORT1, 3, kPORT_MuxAlt4);
    PORT_SetPinMux(PORT1, 12, kPORT_MuxAlt4);
}
void stepper_enable(struct stepper_device_t *ins)
{
    ins->en = STEPPER_EN_ENABLED;
    rt_pin_write(ins->en_pin, PIN_LOW);
}

void stepper_disable(struct stepper_device_t *ins)
{
    ins->en = STEPPER_EN_DISABLED;
    rt_pin_write(ins->en_pin, PIN_HIGH);
}

void stepper_dir_set(struct stepper_device_t *ins, enum stepper_dir_t dir)
{
    ins->dir = dir;
    if (dir == STEPPER_DIR_CW)
        rt_pin_write(ins->dir_pin, PIN_LOW);
    if (dir == STEPPER_DIR_CCW)
        rt_pin_write(ins->dir_pin, PIN_HIGH);
}

void stepper_dir_reverse(struct stepper_device_t *ins)
{
    ins->dir = 1 - ins->dir;
    if (ins->dir == STEPPER_DIR_CW)
        rt_pin_write(ins->dir_pin, PIN_LOW);
    if (ins->dir == STEPPER_DIR_CCW)
        rt_pin_write(ins->dir_pin, PIN_HIGH);
}

rt_err_t motor_x_cb(rt_device_t dev, rt_size_t size)
{
    rt_pwm_disable((struct rt_device_pwm *)motor_x.pwm_device, motor_x.pwm_channel);
    motor_x.motor_lock = 0;
}

rt_err_t motor_y_cb(rt_device_t dev, rt_size_t size)
{
    rt_pwm_disable((struct rt_device_pwm *)motor_y.pwm_device, motor_y.pwm_channel);
    motor_y.motor_lock = 0;
}